from HitbotInterface import HitbotInterface
import time

class FourArm:
    INVALID_INSTRUCTION = 0
    EFFECTIVE_INSTRUCTION = 1
    INVALID_SPEED = 2
    NOT_INITIALIZED = 3
    UNREACHABLE_TARGET = 4
    SERVO_NOT_ENABLED = 6
    MOBILE_CONTROL_ACTIVE = 11
    PARAMETER_NOT_A_NUMBER = 101
    COLLISION_OCCURRED = 102
    AXIS_RESET_OCCURRED = 103

    HOME_POSITION = (420, 0, 0, 0, 70, 0.4, 1) #初始位置
    INSPECTION_POSITION = (418, 0.50, -239.01, 101.07, 70, 0.4, 1) #质检位置

    CONNECTION_TIMEOUT = 10 #设置的服务器连接时间
    SERVER_CONNECT_SUCCESS = 1 
    ARM_INITIALIZATION_SUCCESS = 1

    def __init__(self, robot_id):
        self.robot_id = robot_id
        self.robot = None
        self.move_state = -1

    def initialize_robot(self):
        self.robot = HitbotInterface(self.robot_id)
        self.robot.net_port_initial()

        start_time = time.time()
        ret = self.robot.is_connect()
        while ret != self.SERVER_CONNECT_SUCCESS:
            time.sleep(0.1)
            ret = self.robot.is_connect()
            elapsed_time = time.time() - start_time
            if elapsed_time > self.CONNECTION_TIMEOUT:
                print("机械臂连接失败：连接超时")
                return 0

        ret = self.robot.initial(1, 400)
        if ret == self.ARM_CONNECT_SUCCESS:
            print("机械臂初始化成功")
        else:
            print("机械臂初始化失败")
        return 1

    def move_home(self):
        self.move_state = self.robot.new_movej_xyz_lr(*self.HOME_POSITION)
        self.__handle_move_state()
        return self.robot.wait_stop()

    def move_inspection_position(self):
        self.move_state = self.robot.new_movej_xyz_lr(*self.INSPECTION_POSITION)
        self.__handle_move_state()
        return self.robot.wait_stop()

    def __handle_move_state(self):
        if self.move_state == self.EFFECTIVE_INSTRUCTION:
            print("本次指令生效，机械臂开始运动")
        elif self.move_state ==  self.INVALID_INSTRUCTION:
            print("正在执行其他指令，本次指令无效")
        elif self.move_state == self.INVALID_SPEED:
            print("设置速度小于等于零")
        elif self.move_state == self.NOT_INITIALIZED:
            print("未初始化")
        elif self.move_state == self.UNREACHABLE_TARGET:
            print("目标点无法到达")
        elif self.move_state == self.SERVO_NOT_ENABLED:
            print("伺服未开启")
        elif self.move_state == self.MOBILE_CONTROL_ACTIVE:
            print("手机端在控制")
        elif self.move_state == self.PARAMETER_NOT_A_NUMBER:
            print("传入参数错误")
        elif self.move_state == self.COLLISION_OCCURRED:
            print("发生碰撞，本次指令无效")
        elif self.move_state == self.AXIS_RESET_OCCURRED:
            print("轴发生复位，需要重新初始化，本次指令无效")

    def disconnect_robot(self):
        self.robot.close_server()

if __name__ == '__main__':
    robot_id = 20  # 传入机械臂的ID
    four_arm = FourArm(robot_id)
    four_arm.initialize_robot()
    four_arm.move_inspection_position()
    four_arm.move_home()
    # four_arm.disconnect_robot()